function data = transformData3DRigid(state, data)

trans = state(1:3);
expmap = state(4:6);
rot = quat2rotmat(expmap2quat(expmap));

data = [rot*data']' + repmat(trans, size(data, 1), 1);
